#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/point.hpp>
// 暂时注释掉缺少的头文件
#include <sensor_msgs/msg/joint_state.hpp>
#include "sensor_msgs/msg/joint_state.hpp" 

#include <std_msgs/msg/float64.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <eigen3/Eigen/Dense>
// #include <eigen3/Eigen/Geometry>
#include <chrono>

using namespace std::chrono_literals;
using namespace Eigen;  // 显式声明Eigen命名空间

/**
 * @class VisualControl
 * @brief 视觉控制节点类，用于通过视觉反馈控制机械臂运动
 * 
 * 该类继承自ROS2的节点基类，实现了基于视觉信息的机械臂位置控制
 */
class VisualControl : public rclcpp::Node {
public:
    VisualControl() : Node("visual_control_node") 
    {
        // 初始化参数
        // sensor_msgs::msg::JointState joint_state_;  // 存储当前关节状态
        // geometry_msgs::msg::Point result_data_;     // 存储视觉处理结果
        position_reference_ = Eigen::Vector3d(0.0, 0.0, 0.0);     // 参考位置，设为零
        velocity_ = Eigen::Vector3d::Zero();                      // 初始速度为零
        dt_ = 0.01; 
        current_position_.x() =0;
        current_position_.y() =0;
        current_position_.z() =0;
        current_visual_ = Eigen::Vector3d::Zero();    
        result_data_ = geometry_msgs::msg::Point(); // 初始化视觉结果数据
        // 依赖于sensor_msgs的订阅
        
        // joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
        //     "/joint_states", 10, std::bind(&VisualControl::joint_state_callback, this, std::placeholders::_1));
        
            
        // 订阅视觉处理结果
        result_sub_ = this->create_subscription<geometry_msgs::msg::Point>(
            "/result3_topic", 10,                    // 订阅视觉结果话题，队列大小为10
            [this](const geometry_msgs::msg::Point::SharedPtr msg) {
                result_data_ = *msg;                 // 存储接收到的视觉处理结果数据
            });

        // 创建发布者，发布位置指令到moveit_servo
        pose_pub_ = create_publisher<geometry_msgs::msg::PoseStamped>("/servo_node/delta_pose_cmds", 10);

        // 初始化TF监听器
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

        // 初始化发布器，用于发布处理结果
        tf_pub_ = this->create_publisher<std_msgs::msg::Float64>("result_data", 10);

        // 使用明确的时间规范启动控制循环（每1000ms执行一次）
        timer_ = this->create_wall_timer(
            1000ms, std::bind(&VisualControl::control_loop, this));
    }

private:
    void visual_callback(void)
    {
        // 从TwistStamped消息中提取当前力
        current_visual_.x() =0.05;
        current_visual_.y() = 0;
        current_visual_.z() = 0;
    }
    // 暂时注释掉依赖于sensor_msgs的函数
    /*
    void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg) {
        joint_state_ = *msg;  // 存储接收到的关节状态数据
    }
    */
    Eigen::Matrix3d calculate_rotation_matrix(const std::vector<double>& joints) {
        // 消除未使用参数警告
        (void)joints;  // 显式忽略未使用的参数
        
        // 示例实现：单位矩阵（需替换为实际的运动学逻辑）
        Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
        // TODO: 替换为基于关节角度的实际计算
        // 当前假设无旋转，简化处理

        return rotation;
    }


    void control_loop()
    {
        // 获取当前TCP位置（通过TF变换）
        geometry_msgs::msg::TransformStamped transform;
        try {
            transform = tf_buffer_->lookupTransform("base_link", "end", tf2::TimePointZero);
            current_position_.x() = transform.transform.translation.x;
            current_position_.y() = transform.transform.translation.y;
            current_position_.z() = transform.transform.translation.z;
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "无法获取TF变换: %s", ex.what());
            return;
        }

        // 计算位置差
        // Eigen::Vector3d position_difference = current_position_ - position_reference_;

        // 计算加速度
        // Eigen::Vector3d acceleration =   ;
        // 更新速度
        // velocity_ +=  0 ;
        // 计算位置增量
        Eigen::Vector3d delta_position = velocity_ * dt_;
        // 计算新的位置
        Eigen::Vector3d new_position = current_position_ - delta_position;

        // 构造并发布消息
        geometry_msgs::msg::PoseStamped pose_cmd;
        pose_cmd.header.stamp = this->get_clock()->now();
        pose_cmd.header.frame_id = "base_link";
        pose_cmd.pose.position.x = new_position.x();
        pose_cmd.pose.position.y = new_position.y();
        pose_cmd.pose.position.z = new_position.z();
        pose_cmd.pose.orientation = transform.transform.rotation;
        pose_pub_->publish(pose_cmd);

        RCLCPP_INFO(this->get_logger(), "New position: x=%.3f, y=%.3f, z=%.3f",
                    new_position.x(), new_position.y(), new_position.z());
    }
    // 成员变量
    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr result_sub_;  // 视觉结果订阅器
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tf_pub_;  // 结果数据发布器
    rclcpp::TimerBase::SharedPtr timer_;  // 定时器，用于定期执行控制循环
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;


    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    geometry_msgs::msg::Point result_data_;     // 存储视觉处理结果

    Eigen::Vector3d current_visual_;          // 当前
    Eigen::Vector3d current_position_;
    Eigen::Vector3d velocity_;               // 速度
    Eigen::Vector3d position_reference_;     // 参考位置
    double dt_;                              // 时间步长
                    
    
};

/**
 * @brief 主函数
 * 
 * 初始化ROS2节点并启动视觉控制
 */       
int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<VisualControl>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
